package com.ecust.ml.cardriving.controller;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Iterator;
import java.util.List;

import com.ecust.ml.cardriving.ivl.InverseReinfocementLearning;
import com.ecust.ml.cardriving.model.Car;
import com.ecust.ml.cardriving.model.CarFactory;
import com.ecust.ml.cardriving.model.Highway;
import com.ecust.ml.cardriving.model.Road;
import com.ecust.ml.cardriving.view.CarDrivingSimulator;
import com.ecust.ml.cardriving.view.HighwayViewer;
import com.fly.lib.util.BooleanUtil;
import com.fly.lib.util.DateUtil;

public class CarDriverController implements Runnable{
	
	public static int lastLaneOn = 0;
	public static int[] lastDistance = new int[3];
	
	public static final int MAX_CAR_NUMBER_IN_ONE_LINE = 5;
	
	/**
	 * 目标小车和其他小车之间的距离值范围 -100 ~ 350
	 */
	public static final int MAX_CAR_DISTANCE = 100;
	public static final int MIN_CAR_DISTANCE = -350;
	public static final int CAR_DISTANCE_THRESHOLD = 50;
	public static final int COLLISION_DISTANCE = Car.CAR_LENGTH;
	
	public static boolean collision = false;
	
	/**
	 * 运行模式：训练模式
	 */
	public static boolean trainMode = false;
	
	/**
	 * 运行模式：自动模式
	 */
	public static boolean autoMode = false;
	
	/**
	 * 训练数据缓存
	 */
	private StringBuffer trainingDataBuffer = new StringBuffer();
	
	private Highway highway;
	
	private HighwayViewer highwayViewer;
	
	private Car targetCar;
	
	private InverseReinfocementLearning irl;
	
	public CarDriverController(Highway highway, HighwayViewer highwayViewer,
			Car targetCar) {
		this.highway = highway;
		this.highwayViewer = highwayViewer;
		this.targetCar = targetCar;
	}

	@Override
	public void run() {
		// 更新小车位置
		this.updateCarsPositionOnRoad();
		
		// 记录冲突
		this.recordCollision();
		
		// 添加新小车
		this.generateNewCars();
		
		// 重画画布
		//highwayViewer.repaint();
		
		int laneOn = targetCar.getLaneOn();
		int[] closestDistance = this.getClosestCarDistance();
		
		// 自动控制小车
		this.auto(laneOn, closestDistance);
		
		// 输出训练数据
		this.outputTrainingSet(laneOn, closestDistance);
	}
	
	/**
	 * 使小车根据训练结果自动驾驶（自动模式下）
	 */
	public void auto(int laneIndex, int[] closestDistance) {
		if(autoMode) {
			if(irl == null) {
				/*
				String[] lines = trainingDataBuffer.toString().split("\n");
				double[][] array = new double[lines.length][4];
				for(int i=0; i<lines.length; i++) {
					String line = lines[i];
					String[] datas = line.split(",");
					for(int j=0; j< datas.length; j++) {
						array[i][j] = Integer.valueOf(datas[j]);
					}
				}
				irl = new InverseReinfocementLearning(new Matrix(array));
				*/
				irl = new InverseReinfocementLearning();
				//irl = new InverseReinfocementLearning("/Users/pleasebugme/Desktop/rightnasty_weight");
			}
			double[][] array = new double[4][1];
			array[0][0] = laneIndex;
			for(int i=0; i<closestDistance.length; i++) {
				array[i+1][0] = closestDistance[i];
			}
			int actionLane = irl.getActionByStateMatrix((int) array[0][0], (int) array[1][0], (int) array[2][0], (int) array[3][0]);
			//System.out.println("输入数据："+array[0][0]+" "+array[1][0]+" "+array[2][0]+" "+array[3][0]+"调整方向："+actionLane);
			turnToRoad(actionLane);
		}
	}
	
	/**
	 * 清理道路上的小车
	 */
	public void cleanCarsOnRoad() {
		Iterator<Road> roads = highwayViewer.getHighway().getRoads().values().iterator();
		while(roads.hasNext()) {
			Road road = roads.next();
			road.getCars().clear();
		}
	}
	
	/**
	 * 输出训练结果{目标小车车道, 左侧车距最小值，中间车距最小值，右侧车距最小值}
	 * @param laneIndex
	 * @param closestDistance
	 */
	private void outputTrainingSet(int laneIndex, int[] closestDistance) {
		if(trainMode) {
			
			
			if(!trainingDataBuffer.toString().equals("")) {
				trainingDataBuffer.append("\n");
			}
			trainingDataBuffer.append(laneIndex+",");
			for(int i=0; i<closestDistance.length; i++) {
				trainingDataBuffer.append(closestDistance[i]+",");
			}
			trainingDataBuffer.deleteCharAt(trainingDataBuffer.length()-1);
			
			lastLaneOn = laneIndex;
			lastDistance = closestDistance;
			
			/*
			// 目标小车车道序号
			int i=1;
			for(; i<laneIndex; i++) {
				trainingDataBuffer.append("0,");
			}
			trainingDataBuffer.append("1,");
			for(i++; i<this.highway.getRoads().size()+1; i++) {
				trainingDataBuffer.append("0,");
			}
			
			// 目标小车道路上与其他小车最近距离
			int startIndex = MIN_CAR_DISTANCE/CAR_DISTANCE_THRESHOLD;
			int maxIndex = MAX_CAR_DISTANCE/CAR_DISTANCE_THRESHOLD;
			int j = startIndex;
			for(; j<closestDistance; j++) {
				trainingDataBuffer.append("0,");
			}
			trainingDataBuffer.append("1,");
			for(j++; j<maxIndex+1; j++) {
				trainingDataBuffer.append("0,");
			}
			trainingDataBuffer.deleteCharAt(trainingDataBuffer.length()-1);
			trainingDataBuffer.append("\n");
			*/
		}
	}
	
	/**
	 * 将训练数据写入文件
	 */
	public void writeTrainingDataToFile() {
		File file = new File("/Users/pleasebugme/Desktop/data"+DateUtil.getString(DateUtil.getCurrentTimestamp().getTime()));
		FileWriter fw;
		try {
			fw = new FileWriter(file);
			fw.write(trainingDataBuffer.toString());
			fw.close();
		} catch (IOException e) {
			e.printStackTrace();
		} finally {
			
		}
		
	}
	
	/**
	 * 计算所有车道上离目标小车最近距离(-7 -> +2) [50为单位]
	 * @return
	 */
	public int[] getClosestCarDistance() {
		
		int[] closestDistances = new int[] {-CarDrivingSimulator.HEIGHT, -CarDrivingSimulator.HEIGHT, -CarDrivingSimulator.HEIGHT, 
				-CarDrivingSimulator.HEIGHT, -CarDrivingSimulator.HEIGHT};
		Iterator<Road> roads = this.highway.getRoads().values().iterator();
		int index = 0;
		while(roads.hasNext()) {
			List<Car> cars = roads.next().getCars();
			for(int i=0; i<cars.size(); i++) {
				Car car = cars.get(i);
				int distance = Car.CAR_RUN_POSITION - car.getPosition();
				if(distance > MAX_CAR_DISTANCE) {
					closestDistances[index] = -CarDrivingSimulator.HEIGHT;
				}else if(Math.abs(distance) < Math.abs(closestDistances[index])) {
					closestDistances[index] = distance;
				}
			}
			index++;
		}
		int[] result = scaleDistance(closestDistances);
		return new int[]{result[1], result[2], result[3]};
	}
	
	/**
	 * 将距离值按比例缩小至规定的距离值(-350/CAR_DISTANCE_THRESHOLD->-7, -300/CAR_DISTANCE_THRESHOLD->-6,..., 100/CAR_DISTANCE_THRESHOLD->+2)
	 * @param distance
	 * @return
	 */
	private int[] scaleDistance(int[] distances) {
		int[] result = new int[distances.length];
		for(int i=0; i<distances.length; i++) {
			int distance = distances[i];
			if(distance > MAX_CAR_DISTANCE) {
				distance = MAX_CAR_DISTANCE;
			} else if(distance < MIN_CAR_DISTANCE) {
				distance = MIN_CAR_DISTANCE;
			}
			if(Math.abs(distance) <= COLLISION_DISTANCE) {
				result[i] = 0;
			} else {
				if(distance > 0) {
					distance -= COLLISION_DISTANCE;
				}else {
					distance += COLLISION_DISTANCE;
				}
				result[i] = distance / CAR_DISTANCE_THRESHOLD - (distance%CAR_DISTANCE_THRESHOLD==0?0:(distance<0?1:-1));
			}
		}
		
		return result;
	}
	
	/**
	 * 使小车在某条道路行驶
	 * @param roadIndex 道路编号（从左到右一次1->5）
	 */
	public void turnToRoad(int roadIndex) {
		if(roadIndex < Highway.OFFROAD_LEFT || roadIndex > Highway.OFFROAD_RIGHT) {
			System.out.println("roadIndex数据非法：" + roadIndex);
			return;
		}
		
		int currentOn = targetCar.getLaneOn();
		int error = roadIndex - currentOn;
		if(error < 0) {
			for(int i=0; i<-error; i++) {
				turnLeft();
			}
		} else if(error > 0) {
			for(int i=0; i<error; i++) {
				turnRight();
			}
		}
	}
	
	/**
	 * 使目标小车往左侧道路行驶
	 */
	public void turnLeft() {
		int laneOn = targetCar.getLaneOn();
		if(laneOn <= 1) {
			laneOn = 1;
		} else {
			laneOn --;
		}
		targetCar.setLaneOn(laneOn);
	}
	
	/**
	 * 使目标小车往右侧道路行驶
	 */
	public void turnRight() {
		int laneOn = targetCar.getLaneOn();
		if(laneOn >= 5) {
			laneOn = 5;
		} else {
			laneOn ++;
		}
		targetCar.setLaneOn(laneOn);
	}
	
	/**
	 * 更新道路上小车位置信息
	 */
	public void updateCarsPositionOnRoad() {
		Iterator<Road> roads = highwayViewer.getHighway().getRoads().values().iterator();
		synchronized (highwayViewer) {
			while(roads.hasNext()) {
				Road road = roads.next();
				List<Car> cars = road.getCars();
				for(int i=0;i<cars.size();i++) {
					Car car = cars.get(i);
					int speed = car.getSpeed();
					int length = targetCar.getSpeed() - speed; 
					int newPosition = car.getPosition() - length;
					if(newPosition <= 0) {
						// 小车超出可视区域，则移除
						cars.remove(i);
						i--;
					}else {
						car.setPosition(newPosition);
					}
				}
			}
		}
	}
	
	/**
	 * 记录冲突
	 */
	public void recordCollision() {
		List<Car> cars = this.highway.getRoads().get(targetCar.getLaneOn()).getCars();
		for(int i=0; i<cars.size(); i++) {
			Car car = cars.get(i);
			if(Math.abs(car.getPosition() - Car.CAR_RUN_POSITION) <= Car.CAR_LENGTH) {
				// 冲突
				collision = true;
				return;
			}
		}
		collision = false;
	}
	
	/**
	 * 根据某种策略产生道路上新小车信息
	 */
	public void generateNewCars() {
		if(BooleanUtil.randomTrue(5)) {
			Car car = CarFactory.getARandomCar();
			// 当前最小间隔内有车，则不产生新车
			Road road = this.highway.getRoads().get(car.getLaneOn());
			List<Car> cars = road.getCars();
			for(int i=0;i<cars.size();i++) {
				if(CarDrivingSimulator.HEIGHT - cars.get(i).getPosition() 
						<= Car.CAR_LENGTH*2.5) {
					//System.out.println("最小距离内有车");
					return;
				}
			}
			
			// 当前车道数量多个某个阈值，则不产生新车
			if(this.highway.getRoads().get(car.getLaneOn()).getCars().size() >= MAX_CAR_NUMBER_IN_ONE_LINE) {
				//System.out.println("当前车道数量多个某个阈值");
				return;
			}
			
			
			this.highway.getRoads().get(car.getLaneOn()).getCars().add(car);
		} else {
			//System.out.println("概率值小于50");
		}
		
	}

}
